package us.oh.k12.wkw.robot.subsystem;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class ClimbingSystem extends SystemBase {

    private Solenoid tiltPlungerRelay;
    private int tiltPlungerFireCount = 0;
    private boolean tiltPlungerExtended = false;
    private Compressor compressor;
    private boolean compressorOn = false;

    public String getName() {
        return "ClimbingSystem";
    }

    public String getSmartDashboardType() {
        return "ClimbingSystem";
    }

    public ClimbingSystem() {

        super("ClimbingSystem");

        try {

            this.tiltPlungerRelay = new Solenoid(3);

            LiveWindow.addActuator("ClimbingSystem", "TiltPlungerRelay", this.tiltPlungerRelay);

            this.compressor = new Compressor(1, 1);

            LiveWindow.addActuator("ClimbingSystem", "Compressor", this.compressor);

            this.compressor.start();

            System.out.println("ClimbingSystem initialized.");

        } catch (Exception anEx) {
            this.error("ClimbingSystem()", anEx);
        }

    }

    public void flushStatus() {

        try {

            SmartDashboard.putNumber("TiltPlungerFireCount", this.tiltPlungerFireCount);

        } catch (Exception anEx) {
            this.error("flushStatus()", anEx);
        }
    }

    public void updateStatus() {

        try {

            if (null != this.compressor) {
                if (this.compressorOn != this.compressor.getPressureSwitchValue()) {
                    this.compressorOn = this.compressor.getPressureSwitchValue();
                    SmartDashboard.putBoolean("CompressorEnabled", this.compressorOn);
                }
            }

        } catch (Exception anEx) {
            this.error("updateStatus()", anEx);
        }
    }

    public void tiltPlungerFire() {
        this.tiltPlungerRelay.set(true);
        this.tiltPlungerFireCount++;
        this.tiltPlungerExtended = true;
        SmartDashboard.putBoolean("TiltPlungerExtended", this.tiltPlungerExtended);
    }

    public void tiltPlungerRetract() {
        this.tiltPlungerRelay.set(false);
        this.tiltPlungerExtended = false;
        SmartDashboard.putBoolean("TiltPlungerExtended", this.tiltPlungerExtended);
    }

    public boolean getTiltPlungerState() {
        return this.tiltPlungerRelay.get();
    }

    protected void initDefaultCommand() {

        try {
            // nothing
        } catch (Exception anEx) {
            this.error("initDefaultCommand()", anEx);
        }
    }

    public void startCompressor() {
        this.compressor.start();
    }

    public void stopCompressor() {
        this.compressor.stop();
    }
}
